#include "cmsis_os.h"
#include "robot_total_mode.h"
#include "nucCommu.h"
#include "OLED.h"
#include "bsp_buzzer.h"
#include "detect_task.h"
#include "referee.h" 
#include "CAN_receive.h"

//***************模式设置相关宏定义***********************//
#define RC_ROBOT_MODE_CHANNEL 0     //右边的拨杆控制模式选择。第一版本里，通过位置的停放实现模式改变，之后可以用位置总变化来控制

//***************任务相关宏定义***************************//
#define ROBOT_MODE_TASK_TIME 10    //10ms执行一次，因为涉及鼠标和键盘状态的读取，采样需要快。

//***************全局变量*******************************//
static const fromGimbalSTM_t *gimbalSTM_p;   //遥控器位置指针，需要初始化
// static const toChassisSTM32_t *nuc_p;  //NUC数据位置 
static enum RobotState_e robotState=RobotState_e_Powerless;  //机器人初始状态

/* 当键盘有任何按键开始按下后，就转换到键盘和鼠标控制了，遥控器控制失效。当遥控器右上拨杆发生
状态改变或者遥控器离线时，清除键鼠记录，重新按遥控器控制为准，
*/ 

void robot_total_mode_task(void const *pvParameters)
{
    //初始化数据指针
    gimbalSTM_p=get_gimbal_stm_point();
    // nuc_p=get_nuc_control_point();
    while (1)
    {
        //*****************************OLED 显示调试******************//
       #ifdef OLED_DEBUG_TOTAL_MODE_ON
        time_passed_debug+=ROBOT_MODE_TASK_TIME;
      if(time_passed_debug>OLED_REFRESH_TIME)
        {
            time_passed_debug=0;
            //*********输出和更新******************
            OLED_printf(0,0,"                    ");
            OLED_printf(1,0,"                    ");
            OLED_printf(2,0,"                    ");
            OLED_printf(3,0,"                    ");
            OLED_printf(4,0,"                    ");
            OLED_printf(0,0,"robot mode:%d",getRobotPresentMode());
            OLED_printf(1,0,"key:%d",keyBoardAndMouseHasChanged);
            OLED_refresh_gram();
            //***********************************
        }
        #endif   
        robotState=(enum RobotState_e)gimbalSTM_p->nextmode;

        //*********************掉线检测*******************************//
        if((toe_is_error(GIMBALSTM32_TOE)))
        {
            robotState=RobotState_e_Powerless;
        }
            
        //*************************************************************
        osDelay(ROBOT_MODE_TASK_TIME);
    }
}


const enum RobotState_e * getRobotPresentMode(void)
{
    return &robotState;
}


void OLED_mode(void)
{
    // OLED_printf(0,0,"Auto:%d",robotIsAuxiliaryTarget());
}    
